#include "stm32f10x.h"               
#include "Delay.h"
#include "OLED.h"
#include "Servo.h"   
#include "nrf24l01.h"  // 添加NRF头文件
#include <math.h>
#include "Interrupt.h" //// 添加中断头文件

// 动态步长参数
#define SMALL_ANGLE      5.0f        // 小角度阈值
#define LARGE_ANGLE     15.0f        // 大角度阈值
#define SMALL_STEP       0.8f        // 小步长
#define LARGE_STEP       7.0f        // 大步长

// 增加控制频率
#define CONTROL_INTERVAL 5           // 控制间隔5ms

// 舵机角度范围
#define SERVO1_MIN      30.0f        
#define SERVO1_MAX     150.0f        
#define SERVO2_MIN       0.0f        
#define SERVO2_MAX     180.0f        

// NRF配置
u8 rxAddr[] = {0x10, 0x20, 0x30, 0x40, 0x53};  // 接收地址(与发送端相同)
u8 nrfChannel = 90;                             // NRF通道

// 自定义舵机角度数据结构 (与发送端一致)
struct servo_angle_data {
    uint16_t servo1_angle;  // 舵机1角度 (放大10倍)
    uint16_t servo2_angle;  // 舵机2角度 (放大10倍)
    uint8_t checksum;       // 校验和
    uint8_t reserved[11];   // 保留字节
};

// 舵机状态结构体
typedef struct {
    float target;       // 目标角度
    float current;      // 当前角度
    float velocity;     // 当前速度
    float max_velocity; // 最大速度
    float acceleration; // 加速度
    float min;          // 最小角度限制
    float max;          // 最大角度限制
} ServoState;

// 初始化舵机状态
static ServoState servo1 = {
    .target = 90.0f, 
    .current = 90.0f,
    .velocity = 0.0f,
    .max_velocity = 5.0f,    // 最大速度
    .acceleration = 0.2f,    // 加速度
    .min = SERVO1_MIN, 
    .max = SERVO1_MAX
};

static ServoState servo2 = {
    .target = 90.0f, 
    .current = 90.0f,
    .velocity = 0.0f,
    .max_velocity = 5.0f,    // 最大速度
    .acceleration = 0.2f,     // 加速度
    .min = SERVO2_MIN, 
    .max = SERVO2_MAX
};

// 计算校验和
uint8_t calculate_checksum(uint16_t s1, uint16_t s2) {
    return (uint8_t)((s1 + s2) & 0xFF);
}

// 验证数据有效性
uint8_t validate_data(struct servo_angle_data *data) {
    uint8_t calc_checksum = calculate_checksum(data->servo1_angle, data->servo2_angle);
    return (calc_checksum == data->checksum);
}

// NRF接收数据处理
void NRF_Receive_Data() {
    struct servo_angle_data rx_data;
    u8 ret = NRF_Rx_Dat((u8 *)&rx_data, sizeof(rx_data));
    
    if(ret == RX_DR) {
        // 验证数据有效性
        if(validate_data(&rx_data)) {
            // 转换为浮点角度
            servo1.target = (float)rx_data.servo1_angle / 10.0f;
            servo2.target = (float)rx_data.servo2_angle / 10.0f;
            
            // 角度保护
            servo1.target = (servo1.target < servo1.min) ? servo1.min : 
                           (servo1.target > servo1.max) ? servo1.max : servo1.target;
            servo2.target = (servo2.target < servo2.min) ? servo2.min : 
                           (servo2.target > servo2.max) ? servo2.max : servo2.target;
            
            OLED_ShowString(0, 0, "NRF RX OK ", OLED_8X16);
        } else {
            OLED_ShowString(0, 0, "NRF CHK ERR", OLED_8X16);
        }
    } else {
        OLED_ShowString(0, 0, "NRF NO DATA", OLED_8X16);
    }
}

// 使用S形曲线算法（Sigmoid函数）
float SigmoidStep(float diff, float max_step) {
    // Sigmoid函数：f(x) = 1 / (1 + e^(-x))
    // 调整参数使曲线更平滑
    float x = diff / (max_step * 2.0f);
    float sigmoid = 1.0f / (1.0f + expf(-x));
    return max_step * sigmoid;
}

// 改进的平滑控制函数
void Servo_SmoothControl() { 
    static float last_diff1 = 0, last_diff2 = 0;
    
    float diff1 = servo1.target - servo1.current;
    float step1 = SigmoidStep(diff1, LARGE_STEP);
    
    // 添加低通滤波，减少突变
    float filtered_diff1 = 0.7f * diff1 + 0.3f * last_diff1;
    last_diff1 = diff1;
    
    if (fabsf(filtered_diff1) > step1) {
        servo1.current += (filtered_diff1 > 0) ? step1 : -step1;
    } else {
        servo1.current = servo1.target;
    }

    float diff2 = servo2.target - servo2.current;
    float step2 = SigmoidStep(diff2, LARGE_STEP);
    
    // 添加低通滤波，减少突变
    float filtered_diff2 = 0.7f * diff2 + 0.3f * last_diff2;
    last_diff2 = diff2;
    
    if (fabsf(filtered_diff2) > step2) {
        servo2.current += (filtered_diff2 > 0) ? step2 : -step2;
    } else {
        servo2.current = servo2.target;
    }
    
    // 驱动舵机
    Servo_SetAngle1((uint16_t)servo1.current);  
    Servo_SetAngle2((uint16_t)servo2.current);
}
 
int main(void)
{
    // 初始化
    OLED_Init();         
    Servo_Init();         
    SPI_NRF_Init();       // 初始化NRF
	  TIM3_Init();          // 调用TIM3初始化函数
    
    // 检查NRF连接
    u8 nrf_ret = NRF_Check();
    if (nrf_ret == SUCCESS) {
        OLED_ShowString(10, 0, "NRF OK", OLED_8X16);
    } else {
        OLED_ShowString(10, 0, "NRF ERR", OLED_8X16);
    }
    
    OLED_ShowString(40, 24, "A:", OLED_8X16);     
    OLED_ShowString(40, 48, "B:", OLED_8X16);     
    OLED_Update();

    // 配置NRF为接收模式
    NRF_RX_Mode(rxAddr, nrfChannel, sizeof(struct servo_angle_data));
    Delay_ms(100);

    // 舵机上电默认中间位置
    Servo_SetAngle1((uint16_t)servo1.current);
    Servo_SetAngle2((uint16_t)servo2.current);

    while (1)  
    {
        // 接收NRF数据
        NRF_Receive_Data();

        // 平滑控制舵机
        Servo_SmoothControl();
        
        // OLED显示
        static uint8_t showCnt = 0;
        if (showCnt >= 8) {
            OLED_ShowNum(60, 24, (uint16_t)servo1.current, 3, OLED_8X16);  // 舵机1当前角度
            OLED_ShowNum(60, 48, (uint16_t)servo2.current, 3, OLED_8X16);  // 舵机2当前角度
            OLED_Update();
            showCnt = 0;
        } else {
            showCnt++;
        }

        // 主循环延时
        Delay_ms(2);
    }
}